import os
import launch
import launch_ros
import launch.event_handlers
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, GroupAction,
                            IncludeLaunchDescription, SetEnvironmentVariable)
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    use_sim_time = LaunchConfiguration("use_sim_time", default="True")
   
    pkg_dir = get_package_share_directory('navi_bringup')
    param_file_path = os.path.join(pkg_dir, 'config', 'navi_params.yaml')
    
    # declare_params_file_cmd = DeclareLaunchArgument(
    #     'params_file',
    #     default_value=param_file_path,
    #     description='Full path to the ROS2 parameters file to use for all launched nodes')
    
    lifecycle_nodes = ['controller_server']
    lifecycle_manager_cmd = launch_ros.actions.Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_navi',
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}])
    navi_controller_cmd = Node(
        package='navi_controller',
        executable='controller_server',
        name='controller_server',
        output='screen',
        parameters=[param_file_path]
    )

    ld = LaunchDescription()
    # ld.add_action(declare_params_file_cmd)
    ld.add_action(navi_controller_cmd)
    ld.add_action(lifecycle_manager_cmd)
    return ld
